{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from math import cos, sin, pi"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "x_i_var = 0.1\n",
    "y_i_var = 0.1\n",
    "h_f = 200"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def sample_normal_distribution(b):\n",
    "    \"\"\"\n",
    "    sample normal distribution with 0 mean, variance b\n",
    "    \"\"\"\n",
    "    rand = np.random.uniform(low=-1.0, high=1.0, size=12)\n",
    "    return b * np.sum(rand) / 6"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "def sample_marker_model(marker_global, marker_img):\n",
    "    \"\"\"\n",
    "    Sample marker model to get robot pose in global frame \n",
    "    given marker pose in global frame & in image plane\n",
    "    Input:\n",
    "        marker_global (np.ndarray), shape 3,\n",
    "        marker_img (np.ndarray), shape 3,\n",
    "    Output:\n",
    "        robot_global (np.ndarray), shape 3,\n",
    "    \"\"\"\n",
    "    x_m, y_m, theta_m = marker_global\n",
    "    x_i, y_i, theta_i = marker_img\n",
    "    # robot orientation\n",
    "    theta_r = theta_m - theta_i\n",
    "    k = np.random.randint(low=0, high=4)\n",
    "    theta_r += k*pi/2\n",
    "    # location\n",
    "    x_i_hat = x_i + sample_normal_distribution(x_i_var)\n",
    "    y_i_hat = y_i + sample_normal_distribution(y_i_var)\n",
    "    x_r = x_m - h_f * (x_i * cos(theta_r) - y_i * sin(theta_r))\n",
    "    y_r = y_m - h_f * (x_i * sin(theta_r) + y_i * cos(theta_r))\n",
    "    \n",
    "    return np.array([x_r, y_r, theta_r]) "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "marker_global = np.zeros(3)\n",
    "marker_img = np.array([2., 0., pi/4])\n",
    "robot_pose = [sample_marker_model(marker_global, marker_img) for i in range(1000)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "def plot_sample(x_t, length=1.):\n",
    "    \"\"\"\n",
    "    Plot a pose\n",
    "    \"\"\"\n",
    "    x, y, theta = x_t\n",
    "    x_tip = x + length * cos(theta)\n",
    "    y_tip = y + length * sin(theta)\n",
    "    plt.plot([x, x_tip], [y, y_tip], 'k-')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 360x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "plt.figure(figsize=(5,5))\n",
    "# plt.xlim(0., 2.)\n",
    "# plt.ylim(0., 2.)\n",
    "plt.gca().set_aspect('equal', adjustable='box')\n",
    "plt.xlabel(\"x (cm)\")\n",
    "plt.ylabel(\"y (cm)\")\n",
    "for x_t in robot_pose:\n",
    "    plot_sample(x_t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
